#include "protocol.h"
#include <QtEndian>  // 用于字节序转换（qFromBigEndian/qToBigEndian）
#include <cstring>   // 用于 memcpy（修复 strict-aliasing 警告）
#include <stdexcept> // 用于抛出异常（解析失败时）

// 保留 namespace Protocol（与头文件一致，消除重复声明错误）
namespace Protocol {

/**
 * @brief 解析控制指令实现
 */
ControlCommand parseControlCommand(const QByteArray& data) {
    ControlCommand cmd;
    // 修复：将 offset 改为 size_t 类型（无符号，与 data.size() 匹配，消除符号不匹配警告）
    size_t offset = 0;

    // 1. 解析指令类型（1字节）
    if (offset + sizeof(cmd.cmd_type) > static_cast<size_t>(data.size())) {
        throw std::invalid_argument("Parse error: Insufficient data for command type");
    }
    std::memcpy(&cmd.cmd_type, data.data() + offset, sizeof(cmd.cmd_type));
    offset += sizeof(cmd.cmd_type);

    // 2. 解析序列号（4字节，网络字节序转主机字节序）
    if (offset + sizeof(cmd.seq_num) > data.size()) {
        throw std::invalid_argument("Parse error: Insufficient data for sequence number");
    }
    cmd.seq_num = qFromBigEndian<quint32>(reinterpret_cast<const uchar*>(data.data() + offset));
    offset += sizeof(cmd.seq_num);

    // 3. 解析参数个数（1字节）
    uint8_t param_count = 0;
    if (offset + sizeof(param_count) > data.size()) {
        throw std::invalid_argument("Parse error: Insufficient data for parameter count");
    }
    std::memcpy(&param_count, data.data() + offset, sizeof(param_count));
    offset += sizeof(param_count);

    // 4. 解析参数列表
    for (uint8_t i = 0; i < param_count; ++i) {
        ControlParam param;

        // 4.1 解析参数类型（1字节）
        if (offset + sizeof(param.type) > data.size()) {
            throw std::invalid_argument("Parse error: Insufficient data for param type");
        }
        std::memcpy(&param.type, data.data() + offset, sizeof(param.type));
        offset += sizeof(param.type);

        // 4.2 解析参数值（根据类型读取不同长度）
        switch (param.type) {
            case ControlParam::Type::INT: {
                int32_t int_value = 0;
                if (offset + sizeof(int_value) > data.size()) {
                    throw std::invalid_argument("Parse error: Insufficient data for int param");
                }
                int_value = qFromBigEndian<int32_t>(reinterpret_cast<const uchar*>(data.data() + offset));
                param.value = int_value;
                offset += sizeof(int_value);
                break;
            }
            case ControlParam::Type::DOUBLE: {
                double double_value = 0.0;
                // 用 memcpy 读取字节（修复 strict-aliasing 警告，避免类型强转违规）
                if (offset + sizeof(double_value) > data.size()) {
                    throw std::invalid_argument("Parse error: Insufficient data for double param");
                }
                std::memcpy(&double_value, data.data() + offset, sizeof(double_value));
                // 转换字节序（double 占 8 字节，通过 uint64_t 中转）
                uint64_t double_bytes = qFromBigEndian<uint64_t>(reinterpret_cast<const uchar*>(&double_value));
                std::memcpy(&double_value, &double_bytes, sizeof(double_value));
                param.value = double_value;
                offset += sizeof(double_value);
                break;
            }
            case ControlParam::Type::STRING: {
                uint32_t str_len = qFromBigEndian<uint32_t>(reinterpret_cast<const uchar*>(data.data() + offset));
                offset += sizeof(str_len);
                if (offset + str_len > data.size()) {
                    throw std::invalid_argument("Parse error: Insufficient data for string param");
                }
                std::string str_value(data.data() + offset, str_len);
                param.value = str_value;
                offset += str_len;
                break;
            }
            default:
                throw std::invalid_argument("Parse error: Unknown param type");
        }

        cmd.params.push_back(param);
    }

    // 5. 解析时间戳（8字节）
    if (offset + sizeof(cmd.timestamp) > data.size()) {
        throw std::invalid_argument("Parse error: Insufficient data for timestamp");
    }
    cmd.timestamp = qFromBigEndian<qint64>(reinterpret_cast<const uchar*>(data.data() + offset));
    offset += sizeof(cmd.timestamp);

    // 6. 解析确认标记（1字节）
    if (offset + sizeof(cmd.need_ack) > data.size()) {
        throw std::invalid_argument("Parse error: Insufficient data for ACK flag");
    }
    std::memcpy(&cmd.need_ack, data.data() + offset, sizeof(cmd.need_ack));
    offset += sizeof(cmd.need_ack);

    return cmd;
}

/**
 * @brief 序列化机器人状态实现
 */
QByteArray serializeRobotState(const RobotState& state) {
    QByteArray data;

    // 1. 序列化故障状态（1字节）
    uint8_t is_fault = state.is_fault ? 0x01 : 0x00;
    data.append(reinterpret_cast<const char*>(&is_fault), sizeof(is_fault));

    // 2. 序列化运动状态（1字节）
    uint8_t is_moving = state.is_moving ? 0x01 : 0x00;
    data.append(reinterpret_cast<const char*>(&is_moving), sizeof(is_moving));

    // 3. 序列化末端位置（x/y/z，用 memcpy 避免 strict-aliasing 警告）
    double pos_x = state.end_position.x();
    double pos_y = state.end_position.y();
    double pos_z = state.end_position.z();

    // 转换 double 为大端字节序（通过 memcpy 中转，不直接强转指针）
    uint64_t pos_x_bytes = 0;
    std::memcpy(&pos_x_bytes, &pos_x, sizeof(pos_x_bytes));
    pos_x_bytes = qToBigEndian(pos_x_bytes);
    data.append(reinterpret_cast<const char*>(&pos_x_bytes), sizeof(pos_x_bytes));

    uint64_t pos_y_bytes = 0;
    std::memcpy(&pos_y_bytes, &pos_y, sizeof(pos_y_bytes));
    pos_y_bytes = qToBigEndian(pos_y_bytes);
    data.append(reinterpret_cast<const char*>(&pos_y_bytes), sizeof(pos_y_bytes));

    uint64_t pos_z_bytes = 0;
    std::memcpy(&pos_z_bytes, &pos_z, sizeof(pos_z_bytes));
    pos_z_bytes = qToBigEndian(pos_z_bytes);
    data.append(reinterpret_cast<const char*>(&pos_z_bytes), sizeof(pos_z_bytes));

    // 4. 序列化末端速度（x/y/z，同位置逻辑）
    double vel_x = state.end_velocity.x();
    double vel_y = state.end_velocity.y();
    double vel_z = state.end_velocity.z();

    uint64_t vel_x_bytes = 0;
    std::memcpy(&vel_x_bytes, &vel_x, sizeof(vel_x_bytes));
    vel_x_bytes = qToBigEndian(vel_x_bytes);
    data.append(reinterpret_cast<const char*>(&vel_x_bytes), sizeof(vel_x_bytes));

    uint64_t vel_y_bytes = 0;
    std::memcpy(&vel_y_bytes, &vel_y, sizeof(vel_y_bytes));
    vel_y_bytes = qToBigEndian(vel_y_bytes);
    data.append(reinterpret_cast<const char*>(&vel_y_bytes), sizeof(vel_y_bytes));

    uint64_t vel_z_bytes = 0;
    std::memcpy(&vel_z_bytes, &vel_z, sizeof(vel_z_bytes));
    vel_z_bytes = qToBigEndian(vel_z_bytes);
    data.append(reinterpret_cast<const char*>(&vel_z_bytes), sizeof(vel_z_bytes));

    // 5. 序列化时间戳（8字节）
    qint64 timestamp_big = qToBigEndian(state.timestamp);
    data.append(reinterpret_cast<const char*>(&timestamp_big), sizeof(timestamp_big));

    // 6. 序列化错误码（4字节）
    int32_t error_code_big = qToBigEndian(static_cast<int32_t>(state.error_code));
    data.append(reinterpret_cast<const char*>(&error_code_big), sizeof(error_code_big));

    return data;
}

} // namespace Protocol